#include "mouse.h"
#include "x86.h"
#include "textgraphics.h"
#include "pixelgraphics.h"

namespace mouse
{
  static point2d _pos(0, 0);
  static bool buttons[3];
  static int cycle;
  static uint8_t mouse_bytes[3];
  int x, y;

  const point2d& pos()
  { return _pos; }

  void irq()
  {
    mouse_bytes[cycle++] = x86::inb(0x60);
    if (cycle == 3)
    {
      cycle = 0;

      if ((mouse_bytes[2] & 0x80) || (mouse_bytes[2] & 0x40)) // If overflow, return
        return;

      /* Buttons */
      buttons[0] = (mouse_bytes[2] & 1);
      buttons[1] = (mouse_bytes[2] & 2) >> 1;
      buttons[2] = (mouse_bytes[2] & 4) >> 2;

      //x = (static_cast<int>(mouse_bytes[2]) & 0xFF) | (static_cast<int>(mouse_bytes[0] & (1 << 4)) << 4);
      //x = ((static_cast<int>(mouse_bytes[2]) & 0xFF) << 1) | (static_cast<int>(mouse_bytes[0] & (1 << 4)) >> (4));

      /* Movement */
      x = static_cast<int>(mouse_bytes[0]);
      y = static_cast<int>(mouse_bytes[1]);

      if (mouse_bytes[2] & 0x10)
        x |= 0xFFFFFF00;
      if (mouse_bytes[2] & 0x20)
        y |= 0xFFFFFF00;

      _pos.x += x;
      _pos.y += y;

      if(_pos.x < 0)
        _pos.x = 0;
      if(_pos.x > static_cast<int>(pixel_graphics::cols()))
        _pos.x = pixel_graphics::cols();
      if(_pos.y < 0)
        _pos.y = 0;
      if(_pos.y > static_cast<int>(pixel_graphics::rows()))
        _pos.y = pixel_graphics::rows();
    }
  }

  inline void mouse_wait(uint8_t a_type) //unsigned char
  {
    uint32_t _time_out=100000; //unsigned int
    if (a_type == 0)
    {
      while(_time_out--) //Data
      {
        if((x86::inb(0x64) & 1)==1)
        {
          return;
        }
      }
      return;
    }
    else
    {
      while(_time_out--) //Signal
      {
        if((x86::inb(0x64) & 2)==0)
        {
          return;
        }
      }
      return;
    }
  }

  inline void mouse_write(int8_t a_write) //unsigned char
  {
    //Wait to be able to send a command
    mouse_wait(1);
    //Tell the mouse we are sending a command
    x86::outb(0x64, 0xD4);
    //Wait for the final part
    mouse_wait(1);
    //Finally write
    x86::outb(0x60, a_write);
  }

  uint8_t mouse_read()
  {
    //Get's response from mouse
    mouse_wait(0); 
    return x86::inb(0x60);
  }

  void init()
  {
#if 0
    uint8_t _status;  //unsigned char

    //Enable the auxiliary mouse device
    mouse_wait(1);
    x86::outb(0x64, 0xA8);
    
    //Enable the interrupts
    mouse_wait(1);
    x86::outb(0x64, 0x20);
    mouse_wait(0);
    _status=(x86::inb(0x60) | 2);
    mouse_wait(1);
    x86::outb(0x64, 0x60);
    mouse_wait(1);
    x86::outb(0x60, _status);
    
    //Tell the mouse to use default settings
    mouse_write(0xF6);
    mouse_read();  //Acknowledge
    
    //Enable the mouse
    mouse_write(0xF4);
    mouse_read();  //Acknowledge
#else
    mouse_wait(1);
    x86::outb(0x64,0xA8);

    mouse_wait(1);
    x86::outb(0x64,0x20);

    unsigned char status_byte;
    mouse_wait(0);
    status_byte = (x86::inb(0x60) | 2);

    mouse_wait(1);
    x86::outb(0x64, 0x60);

    mouse_wait(1);
    x86::outb(0x60, status_byte);

    mouse_write(0xF6);
    mouse_read();

    mouse_write(0xF4);
    mouse_read();
#endif
  }
}
